#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include <ros/ros.h>
#include <mbf_abstract_core/abstract_controller.h>
#include <mbf_abstract_nav/MoveBaseFlexConfig.h>
#include <mbf_abstract_nav/abstract_controller_execution.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/TransformStamped.h>
#include <string>
#include <vector>

using geometry_msgs::PoseStamped;
using geometry_msgs::TransformStamped;
using geometry_msgs::Twist;
using geometry_msgs::TwistStamped;
using mbf_abstract_core::AbstractController;
using mbf_abstract_nav::AbstractControllerExecution;
using mbf_abstract_nav::MoveBaseFlexConfig;
using testing::_;
using testing::Return;
using testing::Test;
// for kinetic
using tf::StampedTransform;
// the plan as a vector of poses
typedef std::vector<PoseStamped> plan_t;

// mocked planner so we can control its output
struct AbstractControllerMock : public AbstractController
{
    // the mocked pure virtual members
    MOCK_METHOD4(computeVelocityCommands, uint32_t(const PoseStamped&, const TwistStamped&, TwistStamped&, std::string&));
    MOCK_METHOD2(isGoalReached, bool(double, double));
    MOCK_METHOD1(setPlan, bool(const plan_t&));
    MOCK_METHOD0(cancel, bool());
};

ros::Publisher VEL_PUB, GOAL_PUB;
TFPtr TF_PTR;

// fixture for our tests
struct AbstractControllerExecutionFixture : public Test, public AbstractControllerExecution
{
    AbstractControllerExecutionFixture()
            : AbstractControllerExecution("a name", AbstractController::Ptr(new AbstractControllerMock()),
                    VEL_PUB, GOAL_PUB, TF_PTR, MoveBaseFlexConfig{}) {}

    void TearDown() override
    {
        // after every test we expect that moving_ is set to false
        // we don't expect this for the case NO_LOCAL_CMD
        if (getState() != NO_LOCAL_CMD) EXPECT_FALSE(isMoving());
        // we have to stop the thread when the test is done
        join();
    }
};

TEST_F(AbstractControllerExecutionFixture, noPlan)
{
    // test checks the case where we call start() without setting a plan.
    // the expected output is NO_PLAN.
    // start the controller
    ASSERT_TRUE(start());
    // wait for the status update
    waitForStateUpdate(boost::chrono::seconds(1));
    ASSERT_EQ(getState(), NO_PLAN);
}

TEST_F(AbstractControllerExecutionFixture, emptyPlan)
{
    // test checks the case where we pass an empty path to the controller.
    // the expected output is EMPTY_PLAN
    // set an empty plan
    setNewPlan(plan_t{}, true, 1, 1);
    // start the controller
    ASSERT_TRUE(start());
    // wait for the status update
    waitForStateUpdate(boost::chrono::seconds(1));
    ASSERT_EQ(getState(), EMPTY_PLAN);
}

TEST_F(AbstractControllerExecutionFixture, invalidPlan)
{
    // test checks the case where the controller recjets the plan.
    // the expected output is INVALID_PLAN
    // setup the expectation: the controller rejects the plan
    AbstractControllerMock &mock = dynamic_cast<AbstractControllerMock &>(*controller_);
    EXPECT_CALL(mock, setPlan(_)).WillOnce(Return(false));
    // set a plan
    plan_t plan(10);
    setNewPlan(plan, true, 1, 1);
    // start the controller
    ASSERT_TRUE(start());
    // wait for the status update
    waitForStateUpdate(boost::chrono::seconds(1));
    ASSERT_EQ(getState(), INVALID_PLAN);
}

TEST_F(AbstractControllerExecutionFixture, internalError)
{
    // test checks the case where we cannot compute the current robot pose
    // the expected output is INTERNAL_ERROR
    // setup the expectation: the controller accepts the plan
    AbstractControllerMock &mock = dynamic_cast<AbstractControllerMock &>(*controller_);
    EXPECT_CALL(mock, setPlan(_)).WillOnce(Return(true));
    // set a plan
    plan_t plan(10);
    setNewPlan(plan, true, 1, 1);
    // set the robot frame to some thing else then the global frame (for our test case)
    global_frame_ = "global_frame";
    robot_frame_ = "not_global_frame";
    // start the controller
    ASSERT_TRUE(start());
    // wait for the status update
    // note: this timeout must be larget then the default tf-timeout
    waitForStateUpdate(boost::chrono::seconds(2));
    ASSERT_EQ(getState(), INTERNAL_ERROR);
}

// fixture making us pass computeRobotPose()
struct ComputeRobotPoseFixture : public AbstractControllerExecutionFixture
{
    void SetUp()
    {
        // setup the transform.
        global_frame_ = "global_frame";
        robot_frame_ = "robot_frame";
#ifdef USE_OLD_TF
        StampedTransform transform;
        transform.child_frame_id_ = robot_frame_;
        transform.frame_id_ = global_frame_;
        transform.stamp_ = ros::Time::now();
#else
        TransformStamped transform;
        transform.header.stamp = ros::Time::now();
        transform.header.frame_id = global_frame_;
        transform.child_frame_id = robot_frame_;
        transform.transform.rotation.w = 1;
#endif
        // todo right now the mbf_utility checks on the transform age - but this does not work for static transforms
        TF_PTR->setTransform(transform, "mama");
    }
};

TEST_F(ComputeRobotPoseFixture, arrivedGoal)
{
    // test checks the case where we reach the goal.
    // the expected output is ARRIVED_GOAL
    // setup the expectation: the controller accepts the plan and says we are arrived
    AbstractControllerMock &mock = dynamic_cast<AbstractControllerMock &>(*controller_);
    EXPECT_CALL(mock, setPlan(_)).WillOnce(Return(true));
    EXPECT_CALL(mock, isGoalReached(_, _)).WillOnce(Return(true));
    // we compare against the back-pose of the plan
    plan_t plan(10);
    plan.back().header.frame_id = global_frame_;
    plan.back().pose.orientation.w = 1;
    // make the toleraces small
    setNewPlan(plan, true, 1e-3, 1e-3);
    // call start
    ASSERT_TRUE(start());
    // wait for the status update
    waitForStateUpdate(boost::chrono::seconds(1));
    ASSERT_EQ(getState(), ARRIVED_GOAL);
}

ACTION(ControllerException) { throw std::runtime_error("Oh no! Controller throws an Exception"); }

TEST_F(ComputeRobotPoseFixture, controllerException)
{
    // setup the expectation: the controller accepts the plan and says we are not arrived.
    // the controller throws then an exception
    AbstractControllerMock &mock = dynamic_cast<AbstractControllerMock &>(*controller_);
    EXPECT_CALL(mock, setPlan(_)).WillOnce(Return(true));
    EXPECT_CALL(mock, isGoalReached(_, _)).WillOnce(Return(false));
    EXPECT_CALL(mock, computeVelocityCommands(_, _, _, _)).WillOnce(ControllerException());
    // setup the plan
    plan_t plan(10);
    setNewPlan(plan, true, 1e-3, 1e-3);
    // call start
    ASSERT_TRUE(start());
    // wait for the status update
    waitForStateUpdate(boost::chrono::seconds(1));
    ASSERT_EQ(getState(), INTERNAL_ERROR);
}

// fixture which will setup the mock such that we generate a controller failure
struct FailureFixture : public ComputeRobotPoseFixture
{
    void SetUp()
    {
        // setup the expectation: the controller accepts the plan and says we are not arrived.
        // it furhermore returns an error code
        AbstractControllerMock &mock = dynamic_cast<AbstractControllerMock &>(*controller_);
        EXPECT_CALL(mock, setPlan(_)).WillOnce(Return(true));
        EXPECT_CALL(mock, isGoalReached(_, _)).WillRepeatedly(Return(false));
        EXPECT_CALL(mock, computeVelocityCommands(_, _, _, _)).WillRepeatedly(Return(11));
        // setup the plan
        plan_t plan(10);
        setNewPlan(plan, true, 1e-3, 1e-3);
        // call the parent method for the computeRobotPose call
        ComputeRobotPoseFixture::SetUp();
    }
};

TEST_F(FailureFixture, maxRetries)
{
    // test verifies the case where we exceed the max-retries.
    // the expected output is MAX_RETRIES
    // enable the retries logic (max_retries > 0)
    max_retries_ = 1;
    // call start
    ASSERT_TRUE(start());
    // wait for the status update: in first iteration NO_LOCAL_CMD
    waitForStateUpdate(boost::chrono::seconds(1));
    ASSERT_EQ(getState(), NO_LOCAL_CMD);
    // wait for the status update: in second iteration MAX_RETRIES
    // bcs max_retries_ > 0 && ++retries > max_retries_
    waitForStateUpdate(boost::chrono::seconds(1));
    ASSERT_EQ(getState(), MAX_RETRIES);
}

TEST_F(FailureFixture, noValidCmd)
{
    // test verfies the case where we don't exceed the patience or max-retries conditions
    // the expected output is NO_VALID_CMD
    // disable the retries logic
    max_retries_ = -1;
    // call start
    ASSERT_TRUE(start());
    // wait for the status update
    waitForStateUpdate(boost::chrono::seconds(1));
    ASSERT_EQ(getState(), NO_LOCAL_CMD);
}

TEST_F(FailureFixture, patExceeded)
{
    // test verfies the case where we exceed the patience
    // the expected output is PAT_EXCEEDED
    // disable the retries logic and enable the patience logic: we cheat by setting it to a negative duration.
    max_retries_ = -1;
    patience_ = ros::Duration(-1e-3);
    // call start
    ASSERT_TRUE(start());
    // wait for the status update
    waitForStateUpdate(boost::chrono::seconds(1));
    ASSERT_EQ(getState(), PAT_EXCEEDED);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "read_types");
    ros::NodeHandle nh;
    // setup the pubs as global objects
    VEL_PUB = nh.advertise<Twist>("vel", 1);
    GOAL_PUB = nh.advertise<PoseStamped>("pose", 1);
    // setup the tf-publisher as a global object
    TF_PTR.reset(new TF());
    TF_PTR->setUsingDedicatedThread(true);
    // suppress the logging since we don't want warnings to polute the test-outcome
    if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Fatal))
    {
        ros::console::notifyLoggerLevelsChanged();
    }
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}
